\documentclass[12pt]{article}
\usepackage[UTF8]{ctex}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{geometry}
\geometry{a4paper, margin=1in}

\title{无人机轨迹跟踪与能量管理一体化：\\ 模型预测控制建模 \\（欧拉角表示）}
\author{}
\date{}

\begin{document}
\maketitle

\section{状态空间与控制向量（基于欧拉角）}

\subsection{状态变量，$\mathbf{x} \in \mathbb{R}^{15}$}
状态向量 $\mathbf{x}$ 包含描述无人机运动与能量状态所需的全部变量。其维度为 15。

\begin{equation*}
\mathbf{x} = 
\begin{bmatrix}
    \mathbf{p} \\
    \mathbf{v}_B \\
    \mathbf{\Theta} \\
    \mathbf{\omega}_B \\
    SOC \\
    P_{fc} \\
    \alpha_{\text{老化}}
\end{bmatrix}
\end{equation*}

各组成部分如下：
\begin{itemize}
    \item $\mathbf{p} = [x, y, z]^T$：无人机在惯性参考系中的位置（3 维）。
    \item $\mathbf{v}_B = [v_x, v_y, v_z]^T$：无人机在机体系下的线速度（3 维）。
    \item $\mathbf{\Theta} = [\phi, \theta, \psi]^T$：描述姿态的欧拉角（滚转、俯仰、偏航）（3 维）。
    \item $\mathbf{\omega}_B = [\omega_x, \omega_y, \omega_z]^T$：无人机在机体系下的角速度（3 维）。
    \item $SOC$：锂电池荷电状态（1 维）。
    \item $P_{fc}$：氢燃料电池的实际输出功率（1 维）。
    \item $\alpha_{\text{decay}}$：量化电池累积老化程度的状态变量（1 维）。
\end{itemize}

\subsection{控制变量，$\mathbf{u} \in \mathbb{R}^{5}$}
控制向量 $\mathbf{u}$ 包含 MPC 在每个采样时刻可直接调节的输入。

\begin{equation*}
\mathbf{u} = 
\begin{bmatrix}
    \Omega_1^2 \\
    \Omega_2^2 \\
    \Omega_3^2 \\
    \Omega_4^2 \\
    P_{fc\_\text{cmd}}
\end{bmatrix}
\end{equation*}

各组成部分如下：
\begin{itemize}
    \item $\Omega_i^2$：第 $i$ 个电机角速度的平方（4 维）。
    \item $P_{fc\_\text{cmd}}$：对燃料电池系统的功率指令（1 维）。
\end{itemize}

\section{状态更新方程（基于欧拉角）}
MPC 通过离散时间的状态更新方程预测系统的未来演化：
\begin{equation}
    \mathbf{x}_{k+1} = f_d(\mathbf{x}_k, \mathbf{u}_k)
\end{equation}
其中函数 $f_d$ 由连续时间非线性动力学 $\dot{\mathbf{x}} = f_c(\mathbf{x}, \mathbf{u})$ 在一个采样周期内的数值积分得到。

\subsection{连续时间动力学模型}
\begin{itemize}
    \item \textbf{位置更新（运动学）：}
    \begin{equation*}
        \dot{\mathbf{p}} = R(\mathbf{\Theta}) \mathbf{v}_B
    \end{equation*}
    其中 $R(\mathbf{\Theta})$ 为由机体系到惯性系的旋转矩阵。

    \item \textbf{线速度更新（牛顿定律）：}
    \begin{equation*}
        m \dot{\mathbf{v}}_B = -m(\mathbf{\omega}_B \times \mathbf{v}_B) + \mathbf{F}_g(\mathbf{\Theta}) + \mathbf{F}_{\text{推力}}(\mathbf{u})
    \end{equation*}

    \item \textbf{姿态更新（欧拉角运动学）：}
    \begin{equation*}
        \dot{\mathbf{\Theta}} = W(\mathbf{\Theta}) \mathbf{\omega}_B
    \end{equation*}
    其中 $W(\mathbf{\Theta})$ 为由机体系角速度到欧拉角速度的变换矩阵：
    \begin{equation*}
        W(\mathbf{\Theta}) = 
        \begin{bmatrix}
            1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\
            0 & \cos\phi & -\sin\phi \\
            0 & \sin\phi/\cos\theta & \cos\phi/\cos\theta
        \end{bmatrix}
    \end{equation*}

    \item \textbf{角速度更新（欧拉方程）：}
    \begin{equation*}
        I \dot{\mathbf{\omega}}_B = -(\mathbf{\omega}_B \times (I \mathbf{\omega}_B)) + \mathbf{M}_{B}(\mathbf{u})
    \end{equation*}

    \item \textbf{能量系统动力学：}
    \begin{align*}
        \frac{d(SOC)}{dt} &= -\frac{I_{\text{电池}}}{Q_{\text{容量}}} \\
        \frac{d(P_{fc})}{dt} &= \frac{1}{\tau_{fc}} (P_{fc\_\text{指令}} - P_{fc}) \\
        \frac{d(\alpha_{\text{老化}})}{dt} &= f(I_{\text{电池}}, SOC)
    \end{align*}
\end{itemize}

\section{代价函数 $J$}
MPC 在每个时刻求解一类最优控制问题，以在预测时域 $N$ 上最小化代价函数 $J$。
\begin{equation}
    \min_{\mathbf{u}_0, \dots, \mathbf{u}_{N-1}} J = \sum_{k=0}^{N-1} L(\mathbf{x}_k, \mathbf{u}_k) + \phi(\mathbf{x}_N)
\end{equation}
其中 $L(\cdot)$ 为阶段代价，$\phi(\cdot)$ 为终端代价。

\subsection{阶段代价 $L(\mathbf{x}_k, \mathbf{u}_k)$}
\begin{equation*}
\begin{split}
L(\mathbf{x}, \mathbf{u}) = & \underbrace{ (\mathbf{p} - \mathbf{p}_{\text{ref}})^T \mathbf{Q} (\mathbf{p} - \mathbf{p}_{\text{ref}}) + (\mathbf{\Theta} - \mathbf{\Theta}_{\text{ref}})^T \mathbf{Q}_{\Theta} (\mathbf{\Theta} - \mathbf{\Theta}_{\text{ref}}) }_{\text{轨迹跟踪}} \\
& + \underbrace{ (\mathbf{u} - \mathbf{u}_{\text{ref}})^T \mathbf{R} (\mathbf{u} - \mathbf{u}_{\text{ref}}) }_{\text{控制代价}} \\
& + \underbrace{ w_{H2} \cdot P_{fc} + w_{\text{decay}} \cdot \dot{\alpha}_{\text{decay}} }_{\text{能源与健康}}
\end{split}
\end{equation*}
其中 $\mathbf{Q}$、$\mathbf{Q}_{\Theta}$ 与 $\mathbf{R}$ 为半正定加权矩阵，$w_{H2}$ 与 $w_{\text{decay}}$ 为标量权重。

\section{硬约束}
这些是在预测时域内任意时刻均需满足的严格不等式约束。

\begin{itemize}
    \item \textbf{执行器约束：}
    \begin{equation*}
        \Omega_{\min}^2 \le \Omega_i^2 \le \Omega_{\max}^2, \quad \forall i \in \{1, 2, 3, 4\}
    \end{equation*}

    \item \textbf{燃料电池约束：}
    \begin{gather*}
        P_{fc\_\min} \le P_{fc} \le P_{fc\_\max} \\
        \left|\frac{P_{fc,k+1} - P_{fc,k}}{\Delta t}\right| \le \text{爬坡速率上限}
    \end{gather*}

    \item \textbf{电池状态约束：}
    \begin{gather*}
        SOC_{\min} \le SOC \le SOC_{\max} \\
        |I_{\text{电池}}| \le I_{\text{电池}\_\max}
    \end{gather*}
\end{itemize}

\end{document}